//广播
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <atomic>
#include<geometry_msgs/PoseStamped.h>
#include<sensor_msgs/NavSatFix.h>
#include<geometry_msgs/TwistStamped.h>
#include<geometry_msgs/Point.h>
#include<std_msgs/UInt64.h>
#include <std_msgs/Bool.h>
#include<sensor_msgs/PointCloud2.h>
#include <mavros_msgs/Altitude.h>
#include <octomap/octomap.h>
#include <octomap_msgs/Octomap.h>
#include <octomap_msgs/conversions.h>
#include "unionsys_core.hpp"
//sender
#include <iostream>
#include <cstring>
#include <arpa/inet.h>
#include <sys/socket.h>
#include <vector>
#include <unistd.h>

#include "UDP.h"
#include "Quadrotor.h"

//声明fastdds发布、ros的订阅类
ros::Subscriber rossub_state, rossub_rcin, rossub_vel, rossub_pose, rossub_gps, rossub_altitude;
// udp
extern RegisterMSN *reg_pub;

Quadrotor sfbd_quadrotor;
UDPManager sfbd_manager;
//#define BROADCAST_PORT 12301
//
//
//#define Server_IP "255.255.255.255"
//#define  Test_Server_IP "192.168.1.102"
//
//int sockfd;
//struct sockaddr_in broadcastAddr;
//const char *broadcastIP = Test_Server_IP; // 广播地址
//unsigned short broadcastPort = BROADCAST_PORT;
//char sendString[] = "Helloworld"; // 要发送的字符串
//int broadcastPermission = 1;

void rosposeloopdata(const nav_msgs::Odometry::ConstPtr &odom) {
    if (sfbd_quadrotor.self_gps_point.x == 0) {
        std::cout << "----->no gps fixed,waiting.." << std::endl;
        return;
    }

    PoseMsg msg;

    msg.seq = std::to_string(1);
    std::cout<<"---->sfbd_quadrotor.formation_number: "<<sfbd_quadrotor.formation_number<<std::endl;
    msg.frame_id = std::to_string(sfbd_quadrotor.formation_number);
    msg.xPoint = std::to_string(sfbd_quadrotor.self_gps_point.x);
    msg.yPoint = std::to_string(sfbd_quadrotor.self_gps_point.y);
    msg.zPoint = std::to_string(sfbd_quadrotor.self_gps_point.z);
    msg.xOrientation = std::to_string(sfbd_quadrotor.current_vel_x);
    msg.yOrientation = std::to_string(sfbd_quadrotor.current_vel_y);
    msg.zOrientation = std::to_string(sfbd_quadrotor.current_vel_z);
    geometry_msgs::Quaternion current_angle;
    geometry_msgs::Vector3 curr_plane_angle;
    current_angle.x = odom->pose.pose.orientation.x;
    current_angle.y = odom->pose.pose.orientation.y;
    current_angle.z = odom->pose.pose.orientation.z;
    current_angle.w = odom->pose.pose.orientation.w;
    curr_plane_angle = reg_pub->toEulerAngle(current_angle);
    msg.wOrientation = std::to_string(curr_plane_angle.z);
    auto data = msg.serialize();

    sfbd_manager.sendToAll(data);
    printf("Message sent.\n");
    // 发送数据
//    sendto(sockfd, data.data(), data.size(), 0, (const struct sockaddr *) &broadcastAddr, sizeof(broadcastAddr));
//    printf("Message sent.\n");
}

int main(int argc, char *argv[]) {
    ros::init(argc, argv, "SDPltDDSPosepub");
    ros::NodeHandle nodeHandle;
    reg_pub->detect_serial_number();

    int sfbd_quadrotorId;
    int sfbd_quadrotorNum = sfbd_quadrotor.get_num();
    

    nodeHandle.param<int>("vehicle_id", sfbd_quadrotorId, -1);
    nodeHandle.param<int>("formation_number", sfbd_quadrotor.formation_number, 1);
    sfbd_quadrotor.set_id(sfbd_quadrotorId);

    printf("sfbd_quadrotorId:%d\n", sfbd_quadrotor.get_id());

    for (int i = 0; i < sfbd_quadrotorNum; ++i)
        sfbd_manager.addSender(sfbd_quadrotor.ipAddresses[i], sfbd_manager.BASE_PORT + sfbd_quadrotorId);
    printf("send_port:%d\n", sfbd_manager.BASE_PORT + sfbd_quadrotorId);
    // 设置循环的频率
    ros::Rate loop_rate(200);

    //先接收ros发布的topic,后dds再发布topic
    rossub_state = nodeHandle.subscribe<mavros_msgs::State>("/mavros/state", 10, &Quadrotor::rosstateloopdata,
                                                            &sfbd_quadrotor);
    rossub_rcin = nodeHandle.subscribe<mavros_msgs::RCIn>("/mavros/rc/in", 10, &Quadrotor::rosrcinloopdata,
                                                          &sfbd_quadrotor);
    rossub_vel = nodeHandle.subscribe<geometry_msgs::TwistStamped>("/mavros/local_position/velocity_local", 10,
                                                                   &Quadrotor::rosvelloopdata, &sfbd_quadrotor);
    rossub_gps = nodeHandle.subscribe<sensor_msgs::NavSatFix>("/mavros/global_position/global", 10,
                                                              &Quadrotor::rosgpsloopdata, &sfbd_quadrotor);
    rossub_altitude = nodeHandle.subscribe<mavros_msgs::Altitude>("/mavros/altitude", 10,
                                                              &Quadrotor::rosaltitudeloopdata, &sfbd_quadrotor);
    rossub_pose = nodeHandle.subscribe<nav_msgs::Odometry>("/mavros/local_position/odom", 10,
                                                           rosposeloopdata);




//    // 创建UDP套接字
//    if ((sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) < 0) {
//        perror("socket() failed");
//        exit(1);
//    }
//
//    // 设置套接字选项以允许广播
//    if (setsockopt(sockfd, SOL_SOCKET, SO_BROADCAST, (void *) &broadcastPermission, sizeof(broadcastPermission)) < 0) {
//        perror("setsockopt() failed");
//        exit(1);
//    }
//
//    // 构造广播地址结构体
//    memset(&broadcastAddr, 0, sizeof(broadcastAddr));
//    broadcastAddr.sin_family = AF_INET;
//    broadcastAddr.sin_addr.s_addr = inet_addr(broadcastIP);
//    broadcastAddr.sin_port = htons(broadcastPort);

    // 发送广播数据  
    // if (sendto(sockfd, sendString, strlen(sendString), 0, (struct sockaddr *)&broadcastAddr, sizeof(broadcastAddr)) != strlen(sendString)) {  
    //     perror("sendto() sent a different number of bytes than expected");  
    //     exit(1);  
    // }  
    //循环回调
    while (ros::ok() && nodeHandle.ok()) {
//        PoseMsg msg;
//        msg.seq = "1";
//        msg.frame_id = "ss";
//        msg.xPoint = "1";
//        msg.yPoint = "1";
//        msg.zPoint = "1";
//        msg.xOrientation = "1";
//        msg.yOrientation = "1";
//        msg.zOrientation = "1";
//        geometry_msgs::Quaternion current_angle;
//        geometry_msgs::Vector3 curr_plane_angle;
//        current_angle.x = 1;
//        current_angle.y = 1;
//        current_angle.z = 1;
//        current_angle.w = 1;
//        curr_plane_angle = reg_pub->toEulerAngle(current_angle);
//        msg.wOrientation = std::to_string(curr_plane_angle.z);
//        auto data = msg.serialize();
//
//        // 发送数据
//        sendto(sockfd, data.data(), data.size(), 0, (const struct sockaddr *) &broadcastAddr, sizeof(broadcastAddr));
////    printf("Message sent.\n");
        ros::spinOnce();
        loop_rate.sleep();
    }
//    printf("Broadcast message sent: %s\n", sendString);

    // 关闭套接字  
//    close(sockfd);

    return 0;
}